#include "MyAction.hh"
#include <math.h>
#include <iostream>
#define STAMPE
#define REALTA

using namespace std;

MyAction::MyAction(sense::Sense* s,camera::Camera* c,std::string cf)
        : sense(s), camera(c), door(cf), move(state,environment,command,cf), middle(state,environment,command,cf), rotate(state,environment,command,cf)
{
    util::ConfigFile conf(cf);
    dummyCycles=DEFAULT_DUMMY_CYCLES;
    dummyC=0;
    cicliVaiVia =0;
    fromRotate = false;
    alreadyRotating=false;
    myNumber = 		conf.getDouble("MYNUMBER");
    door_distance=	conf.getDouble("DOOR_DISTANCE");
    lin_vel_final=	conf.getDouble("LIN_VEL_FINAL");
    velocitaFinale=	conf.getDouble("VELOCITAFINALE");
    angoloFinaleCono=	conf.getInt("ANGOLOFINALECONO");
    distanzaFinale = 	conf.getDouble("DISTANZAFINALE");
    fase = 0; //fase centrale
    cicli=0;
    numVisto = -1;
    for (int i=0; i<10 ;i++) numVolte[i]=0;


    // Configuration file conf can be used to read configuration parameters,
    // e.g. related to the behaviors.

    MACRO_PROFILER_CONSTRUCTOR("MyAction")
    MACRO_LASERPLOT_CONSTRUCTOR("MyAction")
}

MyAction::~MyAction() {
    std::cout << __FUNCTION__ << ": destroying MyAction" << std::endl;
    MACRO_PROFILER_DESTRUCTOR
    MACRO_LASERPLOT_DESTRUCTOR

}

//per trovare la distanza dei punti dello scan
double MyAction::lenghtOfScanRange(line::point_type p) {
    return sqrt((p.x*p.x)+(p.y*p.y));
}

// double MyAction::distance(line::point_type p1,line::point_type p2)
// {
//   return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y));
// }


inline void MyAction::update() {

    std::cout << __FUNCTION__ << ": action update" << std::endl;
    // It reads the state (sensor data) and sends the command to motor
    // Objects state and command are members of parent class Action!
    MACRO_PROFILER_LOG_BEFORE("sense->update()")
    sense->update(command, state);
    MACRO_PROFILER_LOG_AFTER("sense->update()")

    MACRO_PROFILER_LOG_BEFORE("camera->getNumbers()")
    camera->getNumbers(state.numbers);
    MACRO_PROFILER_LOG_AFTER("camera->getNumbers()")

    MACRO_LASERPLOT_GNUPLOT(state.ranges)
}


inline void MyAction::act() {

    HoughDoorDetector::door_vector doors;
    line::point_vector scan;
    line::point_type p;

    double minScan=100;



    // Converts scan ranges into points...
    double angleRes = M_PI/180.0 * 0.5;
    double angleBeg = M_PI/180.0 * (-90.0);
    for (unsigned int i = 90; i < 450; i++) {			//CONTROLLARE I VALORI DA CONSIDERARE
        p.x = state.ranges[i] * cos(angleRes*i+angleBeg);
        p.y = state.ranges[i] * sin(angleRes*i+angleBeg);

        scan.push_back(p);
    }
    rotate.setFarthestPointDistance(lenghtOfScanRange(scan[200])); //gli passo uno scan sulla sinistra
    dummyC++;
 /*   if (dummyC==2000) {
        fase = 3 ;
        dummyC=0;
    }
*/
    // scelta della fase di gioco
    switch (fase) {
    case 0: //fase centrale

        if (!state.numbers.empty()) { //se ci sono numeri
#ifdef STAMPE
            cout<<"HO VISTO "<< state.numbers.size() <<" numero(/i)"<<endl;  //dovrebbe essere sempre uno
#endif
            for (list<camera::Number>::const_iterator i=state.numbers.begin(); i != state.numbers.end(); i++)
            {
                numVolte[i->number]++;
#ifdef STAMPE
                cout << "ho visto il NUMERO	 " << i->number <<" alla DISTANZA: "<<i->distance<<"per "<<numVolte[i->number]<<" volte"<<endl;
#endif
                if (numVolte[i->number] >= 3) {

                    for (int index=0; index<10 ; index++)  numVolte[index]=0;
                    if (i->number==myNumber) {
#ifdef STAMPE
                        cout<< "ma è il mio numeroooooooooooooooooooooooooooooooooooooooooooo"<<endl;
#endif
#ifdef REALTA
                        fase=1;
                        dummyC=0;
                        cout<<"___________FASE FINALEEEEE____________"<<endl;
#endif

                    }
                    else {	 //se non è il mio numero
#ifdef REALTA
                        // if (!alreadyRotating) {

                        if (numVisto != i->number && numVisto!=-1)  {
                            // alreadyRotating=true;
                            fase =3;  //ultimo CAMBIAMENTO DOPO I test
                            dummyC=0;
                        }
                        else {
                            fase=2;
                            dummyC=0;
#ifdef STAMPE
                            cout<< "non è il mio numero e continuo a ruotare"<<endl;
#endif
                        }
                        /*  } else {
                        #ifdef STAMPE
                              cout<< "VAI VIA!"<<endl;
                        #endif
                              fase=3;
                              alreadyRotating=false;
                          }*/
#endif
                    }
                    numVisto = i->number;
                } // END IF visto almeno 3 volte

            }// END-FOR per ogni numero visto
        }
        else {		//quando non vedo numeri per 10 step allora sono sicuro che non c'è effettivamente nulla e esploro
        		if (fromRotate == true) {
        		fase = 2;
						fromRotate = false;
						cout<<"GAY"<<endl;
						break;
						}
        		
            dummyCycles++;
            if (dummyCycles >= DEFAULT_DUMMY_CYCLES && fromRotate == false) {
                door.findDoors(scan,doors);
                dummyCycles=0;
                if (!doors.empty() ) {
                    //scelgo la porta su cui dirigermi LA PIU VICINA
                    int doorIndex=0;
                    for (int i=0; i != doors.size(); i++)
                        if (lenghtOfScanRange(doors[i].center) < lenghtOfScanRange(doors[doorIndex].center))  doorIndex=i;
#ifdef STAMPE
                    cout<<"vedo "<< doors.size()<<" porte, la piu vicina è: "
                        <<"D "<<lenghtOfScanRange(doors[doorIndex].center)<<endl
                        <<"x = "<<doors[doorIndex].center.x<<endl
                        <<"y = "<<doors[doorIndex].center.y<<endl;
#endif

//                             line::pose_type closestDoorPose;
                    if  (lenghtOfScanRange(doors[doorIndex].center)<door_distance) {
// 	    closestDoorPose.setPose(doors[doorIndex].first.x,doors[doorIndex].first.y, doors[doorIndex].thetaNorm);
// 	    move.setGoal(closestDoorPose);
// 	    move.act();
                        cout<<"ruuoto accazzum"<<endl;
                        fase=2;
                        dummyC=0;
                    } else { //ci sono porte ma lontane
                        cout<<" MIDDLE ignorando le porte lontane"<<endl;
                        move.act();
                    }
                }
                else  { //non ci sono porte
                    cout<<"MIDDLE"<<endl;
                    move.act();
                }

            } // endif duumyCycles

        } //end else quado non vedo nulla

        break;


//___________________________________________________________________________________
    case 1: //fase finale

        dummyC=0;

        command.rotVel=0;

        for (int i=250 ; i<=290; i++) {
            if (  lenghtOfScanRange(scan[i]) < minScan ) minScan = lenghtOfScanRange(scan[i]) ;
        }
        if (minScan >= 0.3)
        {
            command.linVel=lin_vel_final;
            cout<<"mi avvicino cautelosamente! minscan= "<<minScan<<endl;
        }
        else
        {
            command.linVel=0;
            cout<<"STOP!______________________________________"<<endl;
            sleep(10);
        }
        
                //se c'è un ostacolo dal lato sinistro
                minScan=100;
                for (int i=(270-angoloFinaleCono) ; i<=260; i++) {
                    if (  lenghtOfScanRange(scan[i]) < minScan ) {
                        minScan = lenghtOfScanRange(scan[i]) ;
                        cout<<"minscan sinistro= "<<minScan<<endl;
                    }
                }
                if (minScan < distanzaFinale) {
                    cout<<"volta a destra"<<endl;
                    command.linVel =0.2;//velocitaFinale;
                    command.rotVel= -0.1;
                } else {

                //se c'è un ostacolo dal lato destro
                minScan=100;
                for (int i=280 ; i<=(270+angoloFinaleCono); i++) {
                    if (  lenghtOfScanRange(scan[i]) < minScan ) {
                        minScan = lenghtOfScanRange(scan[i]) ;
                        cout<<"minscan destro= "<<minScan<<endl;
                    }
                }

                if (minScan < distanzaFinale) {
                    cout<<"volta a sinistra"<<endl;
                    command.linVel =0.2;// velocitaFinale;
                    command.rotVel= 0.1;
               }
               }
        
        break;


        //___________________________________________________________________________________
    case 2: //fase rotate
				fromRotate = true;
//controllo su numero di giri nel caso nn veda nessun numero
        cicli++;
        cout<<"cicli=	"<<cicli<<endl;
        if (cicli>=500) {
            fase=3;
            dummyC=0;
            cout<<"HO FINITO I CICLI PASSO A VAI VIA"<<endl;
            cicli=0;
        }



        rotate.setMode(1);
        rotate.act();
        if (!state.numbers.empty()) {
            fase=0;
            dummyC=0;
        }
        break;


        //_________________________________________________________________________________
    case 3: //fase vai via

				fromRotate = false;
        rotate.setMode(2);
        rotate.act();
        if (lenghtOfScanRange(scan[270])> 3 ) {
            cicliVaiVia++;
            cout<<"ciclivaivia = "<<cicliVaiVia<<endl;
            if (cicliVaiVia>=5) {

                fase =4;
                dummyC=0;
                cicliVaiVia=0;
            }
        }
        break;


        //____________________________________________________________________________________
    case 4:   //fase vai via middle
        //non ci sono numeri e ho appena iniziato ad esplorare
        cicliVaiVia++;
        if (cicliVaiVia<=50)  {
            cout<<"MIDDLE vai via"<<endl;
            move.act();


        }
        else {
            fase=0;
            dummyC=0;
            cicliVaiVia=0;
            numVisto=-1;
        }
        break;





    default :
        cerr<<"switch case fallito"<<endl;
    } //fine switch

} // END ACT


